#include "Game.h"
#include <math.h>
#include <Windows.h>
#include <iostream>



#pragma warning (disable : 4482)


Game::Game() :		m_circleId(0), start(0), bPeriodEnd(false), m_totalObjects(0), m_elasticity(0.2f), m_friction(0.02f),
									m_airjetForce(0.0f), massId(0), mAppliedAirJet(true), mAirjetMagnitude(0.0f), mPause(false)
{

	bRope = false;

	setElasticity(0.8f);
	setFriction(0.02f);
	Primitive::setGame(this);

	// init rope
	int ropePoints = 8;
	m_rope.init(10.0f, ropePoints, Vector(-4,12), Vector(4,12), true, true);
	
	// Initial values for components' frequencies
	m_physicsFreq = 200;
	m_networkFreq = 50;
	m_graphicsFreq = 60;

	
	createLines();
	m_circles.reserve(50);
	m_manifold = new ContactManifold();
}

Game::~Game(void)
{
	for (unsigned int i=0; i<m_circles.size(); ++i)
		delete m_circles[i];

	delete m_manifold;
}


void	Game::checkData(std::string msg)
{
	bool nan = false;

	for (unsigned int i=0; i<m_circles.size(); i++) 
	{
		// NaN position
		if (isNaN(m_circles[i]->GetPos()) || isNaN(m_circles[i]->GetVel())) {  // pos, vel
			nan = true;
		}
	}

	if (nan)
		std::cout << "NaN ! !  : " << msg.c_str() << std::endl;
}


void	Game::toggleRope()
{
	bRope = !bRope;

	if (bRope) {
		// remove first 4 lines
		mLines.erase(mLines.begin(), mLines.begin()+4);

		// insert rope lines
		for (int i=0; i<7; i++) {
			Line l;
			Vector p0 = *(m_rope.point(i+0).ptrPos);
			Vector p1 = *(m_rope.point(i+1).ptrPos);
			l.SetPos(0, p0.GetX(), p0.GetY());
			l.SetPos(1, p1.GetX(), p1.GetY());
			l.SetNormal(0,1);
			l.setInvInertia(0.0f);
			l.setInvMass(0.0f);
			mLines.insert(mLines.begin(), l); 
		}
	}
	else { // rope -> static line
		mLines.erase(mLines.begin(), mLines.begin()+7);
		
		
		// Insert static line 
		Line lines[2];
		lines[0].SetPos(-4, 12, 4, 12, 0, 1);		// middle	+
		lines[1].SetPos(-4, 12, 4, 12, 0, -1);		// middle	-

		std::vector<Line> newLines;
		for (int i=0; i<2; i++) {
			lines[i].createSegments(2, newLines);
		}

		for (unsigned int i=0; i<newLines.size(); i++) {
			newLines[i].updateCell();
			mLines.insert(mLines.begin(), newLines[i]);
		}
	}
}

void	Game::createLines()
{
	Line lines[16];
	lines[0].SetPos(-4, 12, 4, 12, 0, 1);		// middle	+
	lines[1].SetPos(-4, 12, 4, 12, 0, -1);		// middle	-
	lines[2].SetPos(20, 20, -20, 20, 0, -1);	// top hor
	lines[3].SetPos(20, 20, 20, -12, -1, 0);	// R ver
	lines[4].SetPos(-20, 20, -20, -12, 1, 0); // L ver
	lines[5].SetPos(20, -12, 12, -12, 0, 1);	// R step
	lines[6].SetPos(-20, -12, -12, -12, 0, 1); // L step
	lines[7].SetPos(12, -12, 4, -20, -1, 1);	// bottom R
	lines[8].SetPos(-12, -12, -4, -20, 1, 1);	// bottom L
	lines[9].SetPos(-4, -20, 4, -20, 0, 1); // bottom
	lines[10].SetPos(-20, -4, -12, -4, 0, 1); // hor self	+
	lines[11].SetPos(-20, -4, -12, -4, 0, -1); // hor self	-
	lines[12].SetPos(-12, -4, -12, 4, 1, 0); // ver self	+
	lines[13].SetPos(-12, -4, -12, 4, -1, 0); // ver self	-
	lines[14].SetPos(4, 4, 12, -4, 1, 1 );	// diag			+
	lines[15].SetPos(4, 4, 12, -4, -1, -1 );	// diag		-
	
	int cuts[16] = { 2, 2, 10, 8, 8, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2 };

	for (int i=0; i<16; i++) {
			lines[i].createSegments(cuts[i], mLines);
	}

	for (unsigned int i=0; i<mLines.size(); i++) {
		mLines[i].updateCell();
		mLines[i].setInvMass(0.0f);
		mLines[i].setInvInertia(0.0f);
	}

	
}

void	Game::Update(float delta)
{
	if (mPause) return;

	if (start == 0)
		startTimer();

	// Process any commands received from clients
	processCommands();
	// Run physics update loop
	SimulationLoop(delta);
}

void	Game::pause()
{
	mPause = !mPause;
}

void	Game::AddCommand(CmdType type, float x, float y, float z)
{
	Command m;
	m.type = type;
	m.x = x;
	m.y = y;
	m_commands.push_back(m);
}

void	Game::AddCircle(float x, float y, float mass)
{
	Vector point(x, y);

	BodyInfo cinfo;
	cinfo.m_pos = Vector(x, y);
	cinfo.m_mass = mass;
	cinfo.m_omega = 0;
	cinfo.m_bActive = true;
	m_circlesInfo[m_circles.size()]  = cinfo;

	Circle* circle = new Circle();
	circle->SetVel(0, 0);
	
	circle->setData(&(m_circlesInfo[m_circles.size()]));
	circle->setInvMass(1.0f / circle->GetMass());
	circle->setInvInertia(1.0f / circle->GetInertia());
	
	m_circles.push_back(circle);

	m_circleId++;
}

void	Game::setAirjet(Vector start, Vector end)
{
	mAirjetStart = start;
	mAirjetEnd = end;
	mAppliedAirJet = false;
}

bool	Game::isNaN(Vector v) 
{ 
	if (v.GetX() != v.GetX() || v.GetY() != v.GetY()) {
		return true; 
	}
	else return false; 
}

void	Game::CalculateObjectPhysics()
{
	for (unsigned int i=0; i<m_circles.size(); ++i)
		m_circles[i]->CalculatePhysics(m_dt);
}

void	Game::DynamicCollisionResponse(float dt)
{
	for(int collision = 0; collision < m_manifold->GetNumPoints(); ++collision)
	{
		ManifoldPoint &point = m_manifold->GetPoint(collision);
		point.contactID1->CollisionResponseWith(point, dt);	
	}
}

void	Game::UpdateObjectPhysics(float dt)
{
	for (unsigned i=0; i<m_circles.size(); i++) {
		m_circles[i]->Update(dt);		
	}
}

bool	Game::atRest(Circle* circle) 
{
	// check support
	bool bOnSupport = false;
	double supportPrc = circle->support / (double)m_physicsFreq;
	if (supportPrc > 0.05) // 5%
		bOnSupport = true;
	

	// check velocity
	bool bNoVel = false;
	if (abs(circle->GetVel().GetX()) < 0.2f && 
		abs(circle->GetVel().GetY()) < 0.2f)
		bNoVel = true;


	if (circle->GetPos().GetX() > 30.0f || circle->GetPos().GetX() < -30.0f ||
		circle->GetPos().GetY() > 30.0f || circle->GetPos().GetY() < -30.0f)
		return true;

	return bOnSupport && bNoVel;
}

void	Game::SimulationLoop(float delta)
{
	m_dt = delta;

	// Update rope's representation
	updateRopeVisuals();

	// Mark inactive objects
	putInactiveAtRest();
	
	//-----------------------------------------------

	// Integrate velocities
	CalculateObjectPhysics();
	
	// Static and dynamic collision detection
	CollisionDetection(m_dt);

	// Resolve collisions
	for (int i=0; i<5; i++) {
		DynamicCollisionResponse(m_dt);
	}
	
	// Clear manifold 
	m_manifold->Assess();
	
	// Apply impulse
	if (!mAppliedAirJet)
		ApplyImpulse();

	// Update rope physics
	m_rope.simulate(delta);

	// Integrate position
	UpdateObjectPhysics(delta);

	//-----------------------------------------------
}

void	Game::ApplyImpulse()
{
	mAirjetMagnitude = (mAirjetStart - mAirjetEnd).length();

	Vector I = mAirjetEnd - mAirjetStart;

	for (unsigned int i=0; i<m_circles.size(); i++) {
		Vector new_vel = (I + m_circles[i]->GetMass() * m_circles[i]->GetVel()) / m_circles[i]->GetMass();
		m_circles[i]->SetVel(new_vel.GetX(), new_vel.GetY());
	}
	
	mAppliedAirJet = true;
}

void	Game::startTimer()
{
    LARGE_INTEGER li;
    QueryPerformanceFrequency(&li);
    freq = static_cast<double>(li.QuadPart);

    QueryPerformanceCounter(&li);
    start = li.QuadPart;

}

double	Game::getTime()
{
	 LARGE_INTEGER li;

    QueryPerformanceCounter(&li);
    __int64 current = li.QuadPart;

    return (double)(current/freq);
}

double	Game::getDeltaTime()
{
    LARGE_INTEGER li;

    QueryPerformanceCounter(&li);
    __int64 current = li.QuadPart;

    double dt = static_cast<double>(current - start) / freq;

    start = current;

    return dt;
}

void	Game::CollisionDetection(float dt)
{

	for (unsigned i=0; i<m_circles.size(); i++) {
		for (unsigned j=i+1; j<m_circles.size(); ++j) { 

			int x1 = (int)m_circles[i]->getCellX();
			int y1 = (int)m_circles[i]->getCellY();

			int x0 = (int)m_circles[j]->cell().GetX();
			int y0 = (int)m_circles[j]->cell().GetY();

			if (abs(x1-x0) > 1  && abs(y1-y0) > 1) {
				continue;
			}

			StaticSphereCollisionDetection(m_circles[i], m_circles[j], this->m_manifold);
			m_circles[i]->CollisionWithCircle(m_circles[j], m_manifold, dt);
		}
	}

	// line-circle static collision detection
	for (unsigned int i=0; i<mLines.size(); i++) {
		for (unsigned int j=0; j<m_circles.size(); ++j) {

			int x0 = (int)mLines[i].getCellX();
			int y0 = (int)mLines[i].getCellY();

			int x1 = (int)m_circles[j]->getCellX();
			int y1 = (int)m_circles[j]->getCellY();

			if (abs(x0-x1) > 1 && abs(y0-y1) > 1)
				continue;

			StaticLineCollisionDetection(&mLines[i], m_circles[j], m_manifold); 
			mLines[i].CollisionWithCircle(m_circles[j], m_manifold, dt);
		}
	}

	// =============
}

bool	Game::StaticLineCollisionDetection(Line* line, Circle* circle, ContactManifold* contactManifold)
{

	if (circle->getActive() == false)
		return false;

	bool loop = false;
	Vector b = circle->GetPos();
	Vector n = line->getNormal();

	float rad = circle->GetRadius();
	Vector p1 = line->GetPos(1);
	Vector p2 = line->GetPos(0);

	// Static line-circle collision detection :
	// Plug circle's eq. into line's equation to find if circle intersects line
	Vector d = p1 - p2;
	Vector f = p2 - b;
	float  r = rad;
	float A = d.dot(d);
	float B = 2.0f*f.dot(d);
	float C = f.dot(f) - r*r;

	float t1 = -1, t2 = -1;

	float discriminant = B*B - 4.0f*A*C;
	if (discriminant < 0)
		return false; // no collision
	else {
		float discriminant_squared = sqrt(discriminant);
		float t1 = (-B + discriminant_squared) / (2.0f * A);
		float t2 = (-B - discriminant_squared) / (2.0f * A);

		float offset=0.0f;

		// two circle-line intersections
		if (t1 >= 0.0f && t1 <= 1.0f || t2 >= 0.0f && t2 <= 1.0f)
		{
			// increase support
			circle->support++;

			// check if both t1,t2 are valid (inside [0,1])
			float v1= -1.0f, v2= -1.0f;
			if (t1 >= 0.0f && t1 <= 1.0f)
				v1 = v2 = t1;
			if (t2 >= 0.0f && t2 <= 1.0f)
				v2 = t2;
			if (v1 == -1.0f)
				v1 = t2;
			// -----------------------------------------

			// v1, v2 averaged
			float t_middle = (v1+v2)/2.0f;

			// intersection point on the line
			Vector middle_point = line->GetPos(0) + (line->GetPos(1)-line->GetPos(0)) * t_middle;

			// vector from circles's center to intersection point
			Vector normal = middle_point - circle->GetPos();

			// middle point-circle = 0 (circle on line)
			if (normal.length() < 0.00001f) {
				Vector new_c = circle->GetPos() + circle->GetRadius() * line->getNormal();
				circle->SetPos(new_c.GetX(), new_c.GetY());
				return true;
			}

			// normalize
			normal = normal.normalise();

			// edge point of circle
			Vector edge_point = circle->GetPos() + normal * circle->GetRadius();

			// displacement t
			float t= (middle_point - edge_point).length();
			t += 0.005f;

			// new position for circle
			Vector new_c = circle->GetPos() - t * normal; // normal looks 'towards' the collision so use -

			Vector q;
			q = circle->GetPos() + normal * circle->GetRadius();
			q.Set(q.GetX(), q.GetY(), t);

			ManifoldPoint point;
			point.impulse = 0.0f;
			point.contactID1 = line;
			point.t_impulse = 0;
			point.contactID2 = circle;
			point.contactPoint = q; // contact Point + t
			point.contactNormal = normal * (-1.0f);
			point.t = -1.0f;
			point.responded = false;

			#ifdef COMMON_RESPONSE
				point.contactID1 = circle;
				point.contactID2 = line;
			#endif

			contactManifold->Add(point);
			return false;

			circle->SetPos(new_c.GetX(), new_c.GetY());
			return true;
		}
	}	

	return false;
}

bool	Game::StaticSphereCollisionDetection(Circle* circle1, Circle* circle2, ContactManifold* contactManifold)
{

	// no check if both circles sleep
	if (circle1->getActive() == false && circle2->getActive() == false)
		return false;

	bool loop = false;
	Vector pos1 = circle1->GetPos();
	Vector pos2 = circle2->GetPos();
	float r1 = circle1->GetRadius();
	float r2 = circle2->GetRadius();

	float dist = pos1.distance(pos2);
	dist -= (r1 + r2);

	if(dist < 0.0f)
	{
		// set support var
		circle1->support++;
		circle2->support++;
		////

		// many balls -> poss. that pos1==pos2 -> colNormal = NaN after normalization
		Vector colNormal = (pos1 - pos2).normalise();

		Vector n1, n2;
		n1 = n2 = colNormal;

		Vector hitPoint = pos1 + circle1->GetRadius() * (pos1-pos2).normalise();
		// Save the penetration value in the unused Z component 
		hitPoint.Set(hitPoint.GetX(), hitPoint.GetY(), -dist);

			ManifoldPoint mpoint;
			mpoint.contactID1 = circle1;
			mpoint.contactID2 = circle2;
			mpoint.contactPoint = hitPoint;
			mpoint.responded = false;
			mpoint.impulse = 0.0f;
			mpoint.t_impulse = 0;
			mpoint.t = -1;
			mpoint.contactNormal = colNormal;
			contactManifold->Add(mpoint);
			
			circle1->setActive(true);
			circle2->setActive(true);
			return false;

		pos1 = pos1 - (n1 * dist * 1.1f);
		pos2 = pos2 + (n2 * dist * 1.1f);


		// critical section
		circle1->SetPos(pos1.GetX(), pos1.GetY());
		circle2->SetPos(pos2.GetX(), pos2.GetY());
		// critical section

		// make sure collided circles are awake
		circle1->setActive(true);
		circle2->setActive(true);

		loop = true;
	}

	return loop;
}

void	Game::processCommands()
{
	float mass[3] = { 1.5f, 2.5f, 3.5f };

	// proccess any commands
	if (m_commands.size()) {
		for (unsigned int i=0; i<m_commands.size(); i++)  
		{
			Command m = m_commands[i];
			
			if (m.type == CmdType::CMD_NEWCIRCLE) {

				AddCircle(m.x, m.y, mass[massId]);
				m_totalObjects++;
				massId = (massId+1) % 3;
			}
			
			else if (m.type == CmdType::CMD_TOGGLESPRING) {
				toggleRope();
			}
			
			else if (m.type == CmdType::CMD_MANYCIRCLES) {
				for (int i=0; i<8; i++) {

					AddCircle(-18.0f + (i+1)*4.5f, 18.0f, mass[massId]);
					m_totalObjects++;
					massId = (massId+1)%3;
				}
			}
		}
		m_commands.clear();
	}
}

void	Game::updateRopeVisuals()
{
	// update lines for rope
	if (bRope) {
		for (unsigned int i=0; i<8-1; i++) {
			mLines[i].SetPos(0, m_rope.getPointPos(i).GetX(), m_rope.getPointPos(i).GetY());
			mLines[i].SetPos(1, m_rope.getPointPos(i+1).GetX(), m_rope.getPointPos(i+1).GetY());
			mLines[i].updateCell();
		}
	}
}

void	Game::putInactiveAtRest()
{
	if (bPeriodEnd) {
		// check which cycles are at rest
		for (unsigned int i=0; i<m_circles.size(); i++) {
			if (m_circles[i]->getActive()) {
				if (atRest(m_circles[i])) {
					m_circles[i]->setActive(false);
				}					
			}
		}
		bPeriodEnd = false;
	}
}


// Getters/Setters
//-------------------------------------------
bool	Game::activeRope()							
{ 
	return bRope; 
}

int		Game::getRopePointsCount()					
{ 
	return m_rope.numPoints();
}

void*	Game::getRopeData()							
{ 
	return m_rope.getDataPtr(); 
}
	
float	Game::getAirjetMagnitude()					
{ 
	return mAirjetMagnitude; 
}
	
void	Game::setElasticity(float v)					
{ 
	Circle::setElasticity(v); 
}

void	Game::setFriction(float v)
{ 
	Circle::setFriction(v); 
}

float	Game::getElasticity() const					
{ 
	return Circle::getElasticity(); 
}

float	Game::getFriction() const						
{ 
	return Circle::getFriction(); 
}
	
float	Game::getAirjetForce() const					
{ 
	return mAirjetMagnitude; 
}

void	Game::setAirjetForce(float v)					
{ 
	m_airjetForce = v; 
}
	
int		Game::getTotalObjects() const					
{ 
	return m_totalObjects; 
}

float	Game::physicsFreq()
{
	return m_physicsFreq;
}

float	Game::networkFreq()
{
	return m_networkFreq;
}

float	Game::graphicsFreq()
{
	return m_graphicsFreq;
}

void	Game::physicsFreq(float v)
{
	m_physicsFreq = v;
}

void	Game::networkFreq(float v)
{
	m_networkFreq = v;
}

void	Game::graphicsFreq(float v)
{
	m_graphicsFreq = v;
}

void	Game::periodEnd(bool v)
{
	bPeriodEnd = v;
}

Rope&	Game::rope()
{
	return m_rope;
}

void*	Game::ptrCircles()
{
	return (void*)&m_circlesInfo;
}